/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/


#include "stdafx.h"
#include "Bullet核心.h"


//#include "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h"


#ifdef _DEBUG 

#define DEF_LIB_PATH "D:/U/DEV/bullet3/lib/Debug/"
#define DEF_LIB_PATH_调试 "Debug"

#else

#define DEF_LIB_PATH "D:/U/DEV/bullet3/lib/Release/"
#define DEF_LIB_PATH_调试 
#endif

#pragma comment(lib, DEF_LIB_PATH "Bullet2FileLoader.lib")
#pragma comment(lib, DEF_LIB_PATH "Bullet3Collision.lib")
#pragma comment(lib, DEF_LIB_PATH "Bullet3Common.lib")
#pragma comment(lib, DEF_LIB_PATH "Bullet3Dynamics.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletCollision.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletDynamics.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletFileLoader.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletInverseDynamics.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletInverseDynamicsUtils.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletRobotics.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletSoftBody.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletWorldImporter.lib")
#pragma comment(lib, DEF_LIB_PATH "BulletXmlWorldImporter.lib")
#pragma comment(lib, DEF_LIB_PATH "ConvexDecomposition.lib")
#pragma comment(lib, DEF_LIB_PATH "LinearMath.lib")



static SolverType gSolverType = SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT;
static int gSolverMode = SOLVER_SIMD | SOLVER_USE_WARMSTARTING |
// SOLVER_RANDMIZE_ORDER |
// SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS |
// SOLVER_USE_2_FRICTION_DIRECTIONS |
0;

static int gNumIslands = 0;

static btScalar gSliderSolverIterations = 10.0f;                                                        // should be int
static btScalar gSliderNumThreads = 1.0f;                                                               // should be int
static btScalar gSliderIslandBatchingThreshold = 0.0f;                                                  // should be int
static btScalar gSliderMinBatchSize = btScalar(btSequentialImpulseConstraintSolverMt::s_minBatchSize);  // should be int
static btScalar gSliderMaxBatchSize = btScalar(btSequentialImpulseConstraintSolverMt::s_maxBatchSize);  // should be int
static btScalar gSliderLeastSquaresResidualThreshold = 0.0f;









static void profileBeginCallback(btDynamicsWorld* world, btScalar timeStep) {
	//gProfiler.begin(Profiler::kRecordInternalTimeStep);
	//std::cout<< timeStep <<"\n";
}

static void profileEndCallback(btDynamicsWorld* world, btScalar timeStep) {
	//gProfiler.end(Profiler::kRecordInternalTimeStep);
	//std::cout << timeStep << "\n";
}







class Profiler
{
public:
	enum RecordType
	{
		kRecordInternalTimeStep,
		kRecordDispatchAllCollisionPairs,
		kRecordDispatchIslands,
		kRecordPredictUnconstrainedMotion,
		kRecordCreatePredictiveContacts,
		kRecordIntegrateTransforms,
		kRecordSolverTotal,
		kRecordSolverSetup,
		kRecordSolverIterations,
		kRecordSolverFinish,
		kRecordCount
	};

private:
	btClock mClock;

	struct Record
	{
		int mCallCount;
		unsigned long long mAccum;
		unsigned int mStartTime;
		unsigned int mHistory[8];

		void begin(unsigned int curTime)
		{
			mStartTime = curTime;
		}
		void end(unsigned int curTime)
		{
			unsigned int endTime = curTime;
			unsigned int elapsed = endTime - mStartTime;
			mAccum += elapsed;
			mHistory[mCallCount & 7] = elapsed;
			++mCallCount;
		}
		float getAverageTime() const
		{
			int count = btMin(8, mCallCount);
			if (count > 0)
			{
				unsigned int sum = 0;
				for (int i = 0; i < count; ++i)
				{
					sum += mHistory[i];
				}
				float avg = float(sum) / float(count);
				return avg;
			}
			return 0.0;
		}
	};
	Record mRecords[kRecordCount];

public:
	void begin(RecordType rt)
	{
		mRecords[rt].begin(mClock.getTimeMicroseconds());
	}
	void end(RecordType rt)
	{
		mRecords[rt].end(mClock.getTimeMicroseconds());
	}
	float getAverageTime(RecordType rt) const
	{
		return mRecords[rt].getAverageTime();
	}
};




static Profiler gProfiler;

class ProfileHelper
{
	Profiler::RecordType mRecType;

public:
	ProfileHelper(Profiler::RecordType rt)
	{
		mRecType = rt;
		gProfiler.begin(mRecType);
	}
	~ProfileHelper()
	{
		gProfiler.end(mRecType);
	}
};












class MySequentialImpulseConstraintSolverMt : public btSequentialImpulseConstraintSolverMt
{
	typedef btSequentialImpulseConstraintSolverMt ParentClass;

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	MySequentialImpulseConstraintSolverMt() {}

	// for profiling
	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE
	{
		//ProfileHelper prof(Profiler::kRecordSolverSetup);
		btScalar ret = ParentClass::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
		return ret;
	}
	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) BT_OVERRIDE
	{
		//ProfileHelper prof(Profiler::kRecordSolverIterations);
		btScalar ret = ParentClass::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
		return ret;
	}
	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) BT_OVERRIDE
	{
		//ProfileHelper prof(Profiler::kRecordSolverFinish);
		btScalar ret = ParentClass::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
		return ret;
	}
	virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) BT_OVERRIDE
	{
		//ProfileHelper prof(Profiler::kRecordSolverTotal);
		btScalar ret = ParentClass::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
		return ret;
	}
};



btConstraintSolver* createSolverByType(SolverType t)
{
	btMLCPSolverInterface* mlcpSolver = NULL;
	switch (t)
	{
	case SOLVER_TYPE_SEQUENTIAL_IMPULSE:
		return new btSequentialImpulseConstraintSolver();
	case SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT:
		return new MySequentialImpulseConstraintSolverMt();
	case SOLVER_TYPE_NNCG:
		return new btNNCGConstraintSolver();
	case SOLVER_TYPE_MLCP_PGS:
		mlcpSolver = new btSolveProjectedGaussSeidel();
		break;
	case SOLVER_TYPE_MLCP_DANTZIG:
		mlcpSolver = new btDantzigSolver();
		break;
	case SOLVER_TYPE_MLCP_LEMKE:
		mlcpSolver = new btLemkeSolver();
		break;
	default:
	{
	}
	}
	if (mlcpSolver)
	{
		return new btMLCPSolver(mlcpSolver);
	}
	return NULL;
}











class MyCollisionDispatcher : public btCollisionDispatcherMt
{
	typedef btCollisionDispatcherMt ParentClass;

public:
	MyCollisionDispatcher(btCollisionConfiguration* config, int grainSize) : btCollisionDispatcherMt(config, grainSize)
	{
	}

	virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher) BT_OVERRIDE
	{
		//ProfileHelper prof(Profiler::kRecordDispatchAllCollisionPairs);
		ParentClass::dispatchAllCollisionPairs(pairCache, info, dispatcher);
	}
};











void myParallelIslandDispatch(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams)
{
	ProfileHelper prof(Profiler::kRecordDispatchIslands);
	gNumIslands = islandsPtr->size();
	btSimulationIslandManagerMt::parallelIslandDispatch(islandsPtr, solverParams);
}











ATTRIBUTE_ALIGNED16(class)
C_物理域 : public btDiscreteDynamicsWorldMt {
	typedef btDiscreteDynamicsWorldMt ParentClass;

protected:
	virtual void predictUnconstraintMotion(btScalar timeStep) BT_OVERRIDE
	{
		ProfileHelper prof(Profiler::kRecordPredictUnconstrainedMotion);
		ParentClass::predictUnconstraintMotion(timeStep);
	}
	virtual void createPredictiveContacts(btScalar timeStep) BT_OVERRIDE
	{
		ProfileHelper prof(Profiler::kRecordCreatePredictiveContacts);
		ParentClass::createPredictiveContacts(timeStep);
	}
	virtual void integrateTransforms(btScalar timeStep) BT_OVERRIDE
	{
		ProfileHelper prof(Profiler::kRecordIntegrateTransforms);
		ParentClass::integrateTransforms(timeStep);
	}

public:
	BT_DECLARE_ALIGNED_ALLOCATOR();

	C_物理域(btDispatcher* dispatcher,
							btBroadphaseInterface* pairCache,
							btConstraintSolverPoolMt* constraintSolver,
							btSequentialImpulseConstraintSolverMt* constraintSolverMt,
							btCollisionConfiguration* collisionConfiguration) : btDiscreteDynamicsWorldMt(dispatcher, pairCache, constraintSolver, constraintSolverMt, collisionConfiguration)
	{
		btSimulationIslandManagerMt* islandMgr = static_cast<btSimulationIslandManagerMt*>(m_islandManager);
		islandMgr->setIslandDispatchFunction(myParallelIslandDispatch);
	}
};




















class C_bt任务计划管理 {
	btAlignedObjectArray<btITaskScheduler*> m_taskSchedulers;
	btAlignedObjectArray<btITaskScheduler*> m_allocatedTaskSchedulers;

public:
	C_bt任务计划管理() {}
	void init() {
		addTaskScheduler(btGetSequentialTaskScheduler());
#if BT_THREADSAFE
		if (btITaskScheduler* ts = btCreateDefaultTaskScheduler())
		{
			m_allocatedTaskSchedulers.push_back(ts);
			addTaskScheduler(ts);
		}
		addTaskScheduler(btGetOpenMPTaskScheduler());
		addTaskScheduler(btGetTBBTaskScheduler());
		addTaskScheduler(btGetPPLTaskScheduler());
		if (getNumTaskSchedulers() > 1)
		{
			// prefer a non-sequential scheduler if available
			btSetTaskScheduler(m_taskSchedulers[1]);
		}
		else
		{
			btSetTaskScheduler(m_taskSchedulers[0]);
		}
#endif  // #if BT_THREADSAFE
	}

	void shutdown() {
		for (int i = 0; i < m_allocatedTaskSchedulers.size(); ++i)
		{
			delete m_allocatedTaskSchedulers[i];
		}
		m_allocatedTaskSchedulers.clear();
	}

	void addTaskScheduler(btITaskScheduler* ts) {
		if (ts) {
#if BT_THREADSAFE
			// if initial number of threads is 0 or 1,
			if (ts->getNumThreads() <= 1)
			{
				// for OpenMP, TBB, PPL set num threads to number of logical cores
				ts->setNumThreads(ts->getMaxNumThreads());
			}
#endif  // #if BT_THREADSAFE
			m_taskSchedulers.push_back(ts);
		}
	}
	int getNumTaskSchedulers() const { return m_taskSchedulers.size(); }
	btITaskScheduler* getTaskScheduler(int i) { return m_taskSchedulers[i]; }
};










static C_bt任务计划管理 g任务计划管理;

S_Bullet物理引擎核心::S_Bullet物理引擎核心() {
	m_线程数 = 8;

	btDiscreteDynamicsWorld* world = 0;
	m_broadphase = new btDbvtBroadphase();
	
	m_调度 = NULL;

	if (m_线程数) {
		if (g任务计划管理.getNumTaskSchedulers() == 0) {
			g任务计划管理.init();
		}

		btDefaultCollisionConstructionInfo cci;
		cci.m_defaultMaxPersistentManifoldPoolSize = 80000;
		cci.m_defaultMaxCollisionAlgorithmPoolSize = 80000;
		//m_collisionConfiguration = new btDefaultCollisionConfiguration(cci);
		m_collisionConfiguration = new btDefaultCollisionConfiguration();

		//m_调度 = new MyCollisionDispatcher(m_collisionConfiguration, 40);
		m_调度 = new btCollisionDispatcherMt(m_collisionConfiguration, 24);
		m_解算方式 = gSolverType;

		btConstraintSolverPoolMt* solverPool;
		{
			SolverType poolSolverType = m_解算方式;
			if (poolSolverType == SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT) {
				// pool solvers shouldn't be parallel solvers, we don't allow that kind of
				// nested parallelism because of performance issues
				poolSolverType = SOLVER_TYPE_SEQUENTIAL_IMPULSE;
			}
			btConstraintSolver* solvers[BT_MAX_THREAD_COUNT];
			int maxThreadCount = BT_MAX_THREAD_COUNT;
			for (int i = 0; i < maxThreadCount; ++i) {
				solvers[i] = createSolverByType(poolSolverType);
			}
			solverPool = new btConstraintSolverPoolMt(solvers, maxThreadCount);
			m_约束解算器 = solverPool;
		}
		btSequentialImpulseConstraintSolverMt* solverMt = NULL;

		

		if (m_解算方式 == SOLVER_TYPE_SEQUENTIAL_IMPULSE_MT) {
			solverMt = new MySequentialImpulseConstraintSolverMt();
		}

		world = new btDiscreteDynamicsWorldMt(m_调度, m_broadphase, solverPool, solverMt, m_collisionConfiguration);
		btAssert(btGetTaskScheduler() != NULL);
	}
	else {
		m_collisionConfiguration = new btDefaultCollisionConfiguration();
		m_调度 = new btCollisionDispatcher(m_collisionConfiguration);

		
		btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
		world = new btDiscreteDynamicsWorld(m_调度, m_broadphase, sol, m_collisionConfiguration);
		
	}
	
	world->setGravity({ 0.0, -9.8, 0.0 });
	world->setInternalTickCallback(profileBeginCallback, NULL, true);
	world->setInternalTickCallback(profileEndCallback, NULL, false);
	world->getSolverInfo().m_solverMode |= gSolverMode | SOLVER_RANDMIZE_ORDER | SOLVER_SIMD;
	world->getSolverInfo().m_numIterations = btMax(1, int(gSliderSolverIterations));
	


	m_物理域空间.push_back(world);
	//world->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);
}

S_Bullet物理引擎核心::~S_Bullet物理引擎核心() {
	auto& World = m_物理域空间[0];

	int32 i;
	for (i = World->getNumConstraints() - 1; i >= 0; i--) {
		World->removeConstraint(World->getConstraint(i));
	}

	for (i = World->getNumCollisionObjects() - 1; i >= 0; i--) {
		btCollisionObject* obj = World->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState()) {
			delete body->getMotionState();
		}
		World->removeCollisionObject(obj);
		delete obj;
	}

	//delete collision shapes
	/*for (int j = 0; j < m_collisionShapes.size(); j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	m_collisionShapes.clear()*/

}




